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ABSTRACT 
The performance of a Kalman filter used to track a hurricane was substantially im- 
proved by implementing a fixed interval smoothing algorithm. This tracking routine was 
designed and implemented in a microcomputer program. Several tracking scenarios were 
simulated and analyzed. Actual storm tracks obtained from the Joint Typhoon Warning 
Center i Guam, Mariana Islands, were used for this research. The application of the 
Kalman tracker to a tropical storm’s wind specd tracking was also investigated by using 


the best track data and observed data. 
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THESIS DISCLAIMER 


The reader is cautioned that computer prograims developed in this research may not 
have been exercised for all cases of interest. While every effort has been made, within 
the time available, to ensure that the programs are free of computational and logic er- 
rors, they cannot be considered validated. Any application of these programs without 


additional verification ts at the risk of the user. 
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1. INTRODUCTION 


" Conceived over warm tropical oceans, born amid torrential thundershowers, and 
nurtured by water vapor drawn inward from far away, the mature tropical cyclone is an 
offspring of the atmosphere with both negative and positive consequences for life. Sc- 
vere cyclones are among the most destructive of all natural disasters, capable of annihi- 
lating coastal towns and killing hundreds of thousands of people. On the positive 
though less dramatic side, they provide essential rainfall over much of lands they cross. 
It is difficult to convey to those who have never experienced a tropical cyclone the hor- 
ror that great hurricanes can bring to ships at sca or people living near the coast. 
Tropical cyclones cause a varicty of damage and the same tropical cyclone often affects 
several nations during its lifetime. They are called “Hurricanes” in the Atlantic and 
eastern Pacific” [Ref. I]. Hurricanes were identified bv female or male names like Pat 
and Tess. These storms will be discussed in this thesis. ‘Tropical cyclones are also 
numbered sequentially according to their starting date. This numbering system 1s used 
with caution when referencing storms from other data bases. 

This thesis attempted to improve the estimation of the hurricane’s future course, 
specd, and position by using a Kalman filter with smoothing. This problem 1s similar 
to the ship tracking problem which 15 discussed in a previous thesis [Wef. 2]. The major 
difference between ship tracking and storm tracking problem is the measurement process 
which is given actual position coordinates (latitude and longitude) in the storm tracking 
problem. llierefore, the lincarization required in the ship tracking problem is unneces- 
sary in the storm tracking problem. The measurement noise varies with the tvpe of the 
sensor (aircraft, satellite, and radar). 

An accurate and reliable method of tracking and targeting is necessary. [he current 
methods used to track a storm include the use of radar, aircraft, and satellite. 11owever, 
the data may or may not be available when needed for a number of reasons. As an ex- 
ample, aircraft may not be available duc to flight restrictions. A Kalman filter with a 
fixed interval smoothing algorithm can be used to track a storm. The smoothing algo- 
rithin is an off-line calculation that uses all measurements taken during a time interval 
O<k< Af to improve the estimate. By having a more accurate assessment of what the 
storm has done in the past, we will be better able to predict ahead and estimate a storm's 


future course, speed, and position. 


The estimation of the wind speed is as important as the storm position estimate. In 
an e(Tort to estimate the possible damage a hurricane's sustained winds and storm surge 
could do to a coastal area, the Kalman filter and the smoother was used to estimate the 
wind speed and to categorize the hurricane. If the wind speed estimate is accurate, а 
hurricane js categorized correctly. This thesis attempts to estimate the hurricane’s future 


wind speed. This will help to design a timely warning system. 


~ 


Ц. PROBEEW STATEMENT 
"A. GENERAL 

The storm-tracking scenario parallels the ship tracking problem in that both prob- 
lems developed a position, course, and speed solution for a target with similar system 
dynamics. The tracking scenario used here involves two storms. The positions of the 
storms are given m x (longitude), and y (latitude) coordinates. This problem will be 
analyzed using state space methods. Given the longitude and latitude (the measurc- 
ments) reccived by a radar, aircraft, or satellite, we are interested in cstimating the lo- 
cation, course, and speed of the storm (the states of the plant). The state vanables for 


WMS PIANC afc x, х, у, апау. 


B. SYSTEM MODEL 


This svstem can be described by the state space cquation 
AN Фи H Wp (2) 


where 
х,= state vector to be estimated, 


$,7 state transition matrix which describes how the states of the dynamic system are 
related, and 


w,= random forcing function with a covariance matrix Q, that 1s defined as 


100 0 0 0 
жары 10040 0 5 
о о 10 0 | а 
0 0 0 100 
The state vector 1s 
X 
X 
Ху a |2 5) 


and the system state equations are 


x ГТО Ше 
x OEO OT 
У krim BO ш k + Ly J (2.4) 
J ОГ? 


C. MEASUREMENT MODEL 


The measurements are linearly related to the state variables, using the measurement 


equation 
Zk = Пуху + vk (2.5) 


Since the x and y position states are observed directly and given by latitude and longi- 


tude position coordinates, the measurement equation can be written as 


x 
2х 1 0 0 01 : 
Bie «i 0 1 9 Ша U (2.0) 

Y 
where the measurement noise у, has a variance associated with the source of the meas- 
urement. In this thesis, mean deviation (nm) of satellite-derived tropical cyclone posi- 
tions from best track positions (PCN values) were used in the calculation of the 
measurement noise covariance matrix for the satellite data. The measurement noise 
covariance matrix values and PCN values are shown in Table 1l. The equation used in 


this calculation is 


Rg = (Mean deviation}? (2.7) 


Table 1. THE MEASUREMENT NOISE COVARIANCE MATRIX VALUĽS 
FOR SATELLITE í 


Lox Mean Deviation 
NE 


у 





The measurement noise covariance matrix values were calculated by using the ac- 
curacy number for the aircraft and radar data. Equation (2.8) was used for aircraft data 


and Equation (2.9) was used for the radar data 


Ку = J (Navigational)? zu (Meteorological)") (2.8) 
R, = (Radar Accuracy)? (2.9) 


where the radar accuracy numbers are shown in Table 2 


Table 2. THE MEASUREMENT NOISE COVARIANCE MATRIX VALUES 


FOR RADAR 










Radar Accuracy 
Ес | қр | те E 
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D. KALMAN FILTER 

Basically, the Kalman filter takes an a priori estimate of the states, projects it ahead 
in time to some predicted estimate, and then calculates a gain vector based on the error 
covariance of these estimates. The error between the observed measurements and the 
predicted measurements of the corresponding state estimates is multiplied by the gain 
vector and the result is added to the predicted state estimates to give the best estimate 
of the true states based on optimal combinations of a prior: estimates and current 
measurements. 

The Kalman filter is the proper algorithm to be used when both the system model 
and the measurement model are linear functions of the state variables. The basic oper- 
ation of the filter 1s a relatively. straightforward recursive process. The equations used 


in the Kalman filter {Ref. 3] are 


Хр = Фик Гик (2-19) 

Zk = Нух, + у, CAN) 

Хе) = Фах (2.12) 

VASA фу а + О, (231.3) 

S Ds D mif. Ry (2.14) 


“ко 3 Хак-о + Са Пк) (2.15) 
Вид“ Ас ое) Ра (2.16) 


where 
Xaa-p ^ projected ahead state estimate, 
Pusey = Projected ahead state error covariance matrix, 
G, = Kalman gain matrix, 
R, = state measurement nolse covariance matrix, and 
l, 


linearized measurement matrix. 


The Kalman gain matrix serves to minimize the mean square estimation error and 
is an indication of how much weight will be placed on the current observation. A large 
gain, indicating a large error covarince, will place more weight on the current observa- 
tion as the filter tries to correct the states. The gain matrix 1s proportional to the vari- 
ance of the uncertainty in the estimate and inversely proportional to the variance of the 


measurement noise. ]t can be expressed as 
Тр-і 


An initial velocity estimate ts taken to be zero since there 15 no velocity information 
at the beginning. Phe initial state. estimates carry with them some error and it is this 
error, or rather an estiniate of this crror, that is Used ta construct tic mma cmon 
covarrance matrix. The initial position error was estimated to be 10 nautical miles in the 
x y direction and the initial velocity was estimated to be 0.158 nautical miles per minute. 
The error was assumed to be zero mean and uncorrelated. With these approximations, 


the initial error covariance matrix 1s given by 


100 0 0 0 
0.025 O 0 

0 0 100 0 

0 0 0 0.025 


TES 


E. SMOOTHING ALGORITHM 
Smoothing 1s a procedure that uses all of the state estimates produced by an esti- 


mator and attempts to improve the accuracy of these estimates by using the negative 


ите dynamics to produce the smoothed estimate. The estimator used here is the 

Kalman filter. The basic idea behind smoothing is that, for a time interval from 0 to & 

(К > Kk) an esumate at time K based on all previous estimates up to time K, ( X, ). 
, will be more accurate than an estimate based only on the estimates up to timc K, (.«, ду 

“ It is a non-real time operation where the available data are processed to obtain an cs- 

timate Xu.” for some past valuc of k ” [Ref 4]. 

Smothing algorithms were categorized into three groups by Meditch {Ref. 5]; 
Fixed Point Smoothing smooths thc estimate xq, at a fixed point k as K increases. 


Fixed Lag Smoothing smooths the estimate x(K — № | К) at a fixed delay N as K in- 
creascs. 


Fixed Interval Smoothing smooths the estimate Xu, oVer the time interval from 0 to 
K where K is fixed and & varies from 0 to K. 

A fixed-iiterval smoothing algorithm was used in this thesis. This smoothing rou- 

tinc provides the optimal state estimate at each time k over a fixed interval from 0 to 


К. The equations used in the smoothing algorithm [Ref. 5] are 


ско 
Ay = Peay? Paci) (2.19) 
Xl. у Xo) T АЦА) — x(k +1] k)) (2.20) 
р 
Род = Ро + ЯК ан — Радуј k (2.21) 


where 
A, = smoothing filter gain matrix, 
Хау = smoothed state estimate a time k based on N observations, and 


Pax = smoothed state error covariance matrix. 


At the beginning of the smoothing, the last filtered estimate becomes the initial 
smoothed estimate. The index Ќ is decremented bv onc for each pass during tlic 
smoothing algorithm with the starting value of k equal to the number of data points to 
be smoothed, minus one (V—1 ). Consequently, the tracking program makes (N — 1) 


passes through the smoothing algorithin. 
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Ш. STORM TRACKING 
"A. GENERAL 


The Kalman filter program STORM.FOR was used in computer simulations. This 
program was originally written for a ship tracking problem and was modified to use on 
storm tracking problem. The graphing routines of the MATLAB were used to generate 
the graphs. A complete listing of the program is included in Appendix A. Typhoon Tess 
and Tvphoon Pat were used for simulations. The storm tracks used were obtuined from 
data collected at the Joint Typhoon Warning Center located in Guam. Lach storm 15 
given a separate deck name. Tropical cyclones are numbered sequentially according to 


their starting date by the JTWC. There are four types of data: 


Best Track -This file 1s the 6-hourly storm positions based on a post storm. 
subjectively smoothed path. 


Forecasts -This data contains the real time storm positions, objective forecasts, and the 
official forecast. Each date-time group may contain one, two, or all three types of 
data. 


Forecast Errors -Eight different errors were computed for cach of the objective and 
official forecasts. 


Fixes - Tropical cvclone fixes (observations) from four different platforms are con- 
tamed in the data base. 

The position coordinates were obtained using aircraft, satellite, and radar. The data 
obtained included: raw data (observations); best track data; and 12, 24, and 48 hour 
predictions. The raw data was processed just as if 1t was real-time observation of the 
hurricanc. The first storm, Pat, originated cast of Tarwan in the western Pacific on 24 
August 1985. The warning period for this storm was six davs. The storm traveled 1337 
nm. The maximum speed of the wind was over 107 kt and the minimum sea level pres- 
sure was 1002 mb. The Typhoon Pat caused significant damage in southwestern and 
northeastern Japan; primarlv on the islands of Kyushu and Hokkaido. Kyushu was hit 
the hardest with wind gusts of 107 kt. A total of 23 people were reported killed with over 
180 people injured. An estimated 3000 homes were damaged. Pat also severely dis- 
rupted transportation by land, sea, and air. 

The second storm track analyzed was that of Typhoon Tess which originated 
southeast of Guan on 30 August 1985. The warning period for this storm was seven 


days. Fhe storm traveled 1470 nm with maxinium wind speeds of over 90 kt. The storm 


brought needed rain to the Philippines during a spell of drier than normal weather. The 
storm also brought death and destruction. Considerable flooding and crop damage oc- 
curred over southern China as Tess moved inland [Ref. 6]. The observed track of 


Typhoon Pat and Typhoon Tess are shown in Figure | and Figure 2, respectively. 
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Figure |. 
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The observed track of Typhoon Pat [Ref. 6 ] 
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The observed track of Typhoon Tess [Ref. 0 
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B. COMPUTER SIMULATIONS 
l. Typhoon Pat 

The best -track of Typhoon Pat ts shown in Figure 3. The best track positions 
are in 6-hourly increments. The first tracking data point corresponds to the day-time 
group 08270000Z. Figure 4 shows the Kalman filter position estimates and Гірше 5 
shows the smoothed position estimates. Figure 6 was constructed using the filtered and 
smoothed position estimates. In general, the smoother does improve the track accuracy. 
In the arca of the track where the true positions do vary, the smoother tracking error is 
Zero. Specifically, this area occurs between 237 IN, 124° Evand 3s №, 1353 Е Пята 
can be seen easily in Figure 7. This figure was constructed by using the tracking error 
of the filter and smoother. The average tracking errors for this storm are c 4 nautical 
nules for the filter and + 2 nautical mules for the smoother estimates. 

2. Typhoon Tess 

The performance of the smoother on the track of Typhoon Tess was similar to 
that of Typhoon Pat. Figure 8 shows Typhoon Tess best track. Typhoon Tess best 
track data are also in 6-hourly increments. The filter and smoother tracking results are 
shown in Figure 9 and Tigure 10, respectively. Figure J] shows the track results ob- 
tained with the Kalman filter and smoothing algorithm. The smoother shows some im- 
provement near 17.5° N, 120° and: 15.2" INV IS0 ES he initer avcrace thachmip icon 
increased slightly, to about +5 nm, but the smoother average tracking error jump to 
about +5 nm. This is because the smoother gives 30 nautical miles tracking error near 
18.8° ~, 116°E due to large change on the directientof Typhoon less) figured? shies 
the tracking errors of the filter and smoother. It 1s observed that the smoother was 
much less sensitive to the large course changes than the Kalman filter. It 1s, therefore, 
reasonable to assume that similar results could be expected from the smoother for a 
large course change more than 90°. Ilowever, the smoother’s estimates are quite good 
over the entire trajectory and the estimates closcly follow’ the course changes as in 


Typhoon Pat. 
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Figure 3. The Best Track of Typhoon Pat 
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Пеше 4. Filtered Track of Typhoon Pat 
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Figure 5. Smoothed Track of Typhoon Pat 
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Filtered and Smoothed Track of Typhoon Pat 
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STORM1 TRACKING ERRORS -- FIL(o) vs SM(x) 
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Tracking Errors of the Filter and Smoother for typhoon Pat 
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The Best Track of Typhoon Tess 


18 


LONGITUDE E--(DEGREE) 


~ 
= 
> 
= 
th 
P 
E 
em 
го 
EL. 
M 
| 
2 
ка 
E 
E 
~ 
= 
x 
о 
E 
2 


24 


Figure 9. 
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Filtered Track of Typhoon Tess 
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Ligure 10. 
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Smoothed Track of Typhoon Tess 
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Figure 11. 
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Filtered and Smoothed Track of Typhoon Tess 
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Figure 12. 
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Tracking Errors of the Filler and Smoother for typhoon Tess 
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IV. STORM WIND TRACKING 
A. GENERAL 


" [n an effort to estimate the possible damage a hurricane’s sustained winds and 
storm surge could do to a coastal area, the SafTir-Simpson damage-potential scale was 
developed. The scale numbers are based on actual conditions at some time during the 


life of the storm " [Ref. 7]. Table 3 shows these categories. 


Tahle 3. SAFFIR-SIMPSON HURRICANE DAMAGE-POTENTIAL SCALE 


Scale Num- Wind Damage 
ber speed(knots) ee 


Damage mainly to trees, shrubbery, and unanchored 
mobile homes. 


Some trees blown down; major damage to exposed 
mobile homes; some damage to roofs of buildings. 


Foliage removed from trees; large trees blown down; 
mobile homes destroyed; some structural damage to 
small buildings. 


All signs blown down; extensive damage to roofs, win- 
dows, and doors; complete destruction of mobile 
homes; flooding inland as far. 


Severe damage to windows and doors; extensive dam- 

age to roofs of homes and industrial buildings: small 

buildings overturned and blown away; major damage 

to lower floors of all structures less than 4.5 m above 
sea level within 500 m of shore. 


The storm wind tracking scenario parallels the storm tracking problem. The track- 
ing scenario used here involves two storms. This problem Will be analyzed using state 
space methods. Given the tropical cyclone intensity values the observed speed of the 
storm wind will be estimated by using the Kalman filter and smoother. Table 4 slrows 
the relationship between intensity and wind speed. The wind speed was used directly as 
a measurement for the best track data of the storm. The state variables for this plant 
агс w, and w. 


The system can be described by the state space equation 


Wray = gag + fk (4.1) 


where 
w,= state vector to be estimated, 


ф, = state transition matrix which describes how the states of the dynamic system are 
related, and | 


№ = random forcing function with a covariance matrix Q, that is defined as 


It 7? 
| Job = 
O,=| 2 (ЕМ (4.2) 
E ге 
2 
The state vector 1s 
2 ki (4.3) 


and the system statc cquations are 


ОКЕ S ІНЕ Li (4.4) 


The measurements are linearly related to the state variables. Using the measurement 


equation 
Zk = Hym, + Vp (3.5) 


The measurement equation can be written as 


да = [10 Js T Ek | (4.0) 


where the measurement noise v, has a variance associated with the source of the meas- 
urement. The measurement noise covariance matrix values are calculated in the same 
manner as in storm position tracking problem by using Equations (2.8) and (2.9) for the 
асга апа таба ааб" 


The initial error covariance matrix used in the wind speed tracking is 


1000000 0 0 0 


Жақа 0 0.25 0 0 :: 
Ше 0 0 1000000 0 ш 
() () 0 0.25 


Table 4. MAXIMUM SUSTAINED WIND SPEED AS A FUNCTION OF 
FORECAST INTENSITY NUMBER 





B. COMPUTER SIMULATIONS í 
1. The Best Track Data 
а. Typhoon Pat 

Using the best track data wind speed values as the measurements, future 
wind speed values were estimated by the filter and the smoother. There is an initial track 
error duc to the error in the initial state estimates. When the wind speed increases at 
24 hours, the tracking error decreases and becomes zero for the fifth data as the filter 
gains the wind track. However, it increases after 90 hours when the wind speed de- 


creases verv fast and it returns to zero two data points later as the filter regains the wind 


track. ligure 14 shows the filter tracking accuracy. Tlie smoother 15 not as accurate as 
in the position estimate duc to the large change in wind speed, but these errors remain 
in the acceptable ranges. The smoother track 15 shown in Figure 15. The average 
tracking errors are + 0.5 mph for the filter and E T-Dpiiorthe smoother este 
data represents the weather service’s estimate of truth (Ref. 6]. Figure 16 compares the 
forward time estimate (filter, FIL(o)) with the forward and negative time estimate 
(smoother, SM(x)) for Typhoon Pat. Figure 17 denotes the error in these estimates. 
b. Typhoon Tess 

The trackmg results for this storm are shown in Figures 18-22. From Figure 
19 and 20, we can see how the Kalman filter and the fixed interval smoothing improve 
the overall track estimate. During the overall track estimate, two large filter tracking 
errors are detected. This is shown in Figure 19. In both instances the smoother also 
gives large tracking errors. Figure 20 shows the smoother estimates. At other times, 
however, the filtered and ’snioothed estimate are accurate m figure 2s the COI SEND 
of the filter and smoother estimates. The filter average tracking error is + 1.5 mph and 
the smoother average tracking error is + 2.0 mph. Figure 22 shows the tracking crrors 


of the filter and smoother estimates. 
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The Best Track Wind Speed of Typhoon Pat [Ref. 6 ] 
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Figure 14. 
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Filtered Track of Typhoon Pat's Best Track Wind Speed 
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STORMI TRACKS - TRU(o) vs 5М(х) 
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Figure 15. 
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Smoothed Track of Typhoon Pat's Best Track Wind Speed 
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STORMI TRACKS - FIL(o) ws SM(x) 
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Iigure 16. — Filtered and Smoothed Track of Typhoon Pat’s Best Track Wind Speed 
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Figure 17. 
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The Best Track Wind Speed of Typhoon Tess [Ref. 6 ] 
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Filtered Track of Typhoon Tess’ Best Track Wind Speed 
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Figure 20. 
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Smoothed Track of Typhoon Tess’ Best Track Wind Speed 
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Figure 21. Filtered and Smoothed Track of Typhoon Tess’ Best Track Wind Speed 
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DATA POINT 


2. The Observed Wiud Speed Data 


There was uncertainity in the observed data obtained from the JTWC [Ref. 6]. 

This data has more than one data at the same time instant for the different positions 

‚ [rom the eye of the hurricane. This is shown in Figures 23 and 24. There was a strong 
potential for the filter to go unstable. This was data smootlied using the. Equations (4.8) 


and (4.9). The data obtained before and after curve fitting is shown in Figures 25 and 
26. 


үш еу 

Пұ-| 1.0 0.0 00 (4.8) 
O 
ORE НЕ 

Пе = Ма (4.9) 


Where 
z,= measurements to be smoothed, and 


х, = smoothed measurements. 


a. Typhoon Pat 
Using the interpolated data as an observed data, tracking results obtained 
for typhoon Pat are shown in Figures 27 and 28. The filter and smoother estimates the 
wind speed accurately. ‘There ts no potential for the filter and smoother to go unstable. 
The accuracy of the filter is about 7095, and the smoother is about 65%. Due to the 
instant change in the wind speed, the smoother cannot adapted to this change easily. 
b. Typhoon Tess 
The performance of the filter and the smoother are better in Tvphoon Tess. 
They estimate the wind speed very accurately. Again, there is no potential for the filter 
and smoother to go unstable. During the tracking scenario the filter gives the actual 
observed valuc and the smoother docs improve thevaccuracy of these estimates. The 
tracking error is usually zero or very close to zero. The accuracy of the filter and 
smoother are almost the same in this hurricane which is about 85%. The tracking re- 


sults are shown in Figures 29 and 30. 
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Figure 23. — The Observed Wind Speed at Some Distance of Typhoon Pat [Ref. 6 | 
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Figure 24. 
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Figure 25. Jhe Observed and Interpolated Track of Typhoon Pat 
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The Observed and Interpolated Track of Typhoon Tess 
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Filtered Track of Typhoon Pat’s Observed Wind Speed 
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STORMI TRACKS - OBS(x) vs SM(o) 
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Smoothed Track of Typhoon Pat’s Observed Wind Speed 
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Figure 29. 
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Filtered Track of Typhoon Tess' Observed Wind Speed 
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Figure 30. 
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Smoothed Track of Typhoon Tess’ Observed Wind Speed 
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V. CONCLUSIONS 


]he purpose of this rescarch was to improve the accuracy and storm tracking ca- 
pability of a Kalman filter tracking by implementing a fixed interval sinoothing algo- 
rithm. Two different tropical storms were simulated and the accuracy of the observed, 
the filtered, and the smoothed storm tracks were analyzed and discussed. 

The fixed interval smoothing algorithm improved the position accuracy of the storm 
in all of the tracking scenarios simulated. ]lowever, the smoothed result was not always 
the most accurate for everv storm track. The smoother did improve the track accuracy 
on the basis of the best track storm positions. The effectiveness of the smoother т- 
creased as the storm hfettmne increased and the storm course change decreased. 

The storm wind speed tracking scheme mnplemented worked well. Ilowever, because 
this tracking involves the addition of a time-varving value of the state excitation matrix, 
Q, , there was a strong potential for the filter to go unstable. I1 his was observed during 
the storm wind speed tracking. It was diflicult to decide the value of OQ, and R, for ob- 
served wind speed tracking, because intensity could not be observed many times. This 
problem was solved by using a curve fitting method and then this data was used for in- 
puts to the tracking problem. The results show that this method can be used to in- 
terpolate the uncertain data and to avoid an unstable filter. 

The application of the Kalman filter tracker to the storm tracking problem would 
be very useful in attempting to predict the storm's track when little data is available, às 
scen in observed wind speed tracking problem. Then, by using the filter and smoothimg 
algorithm, track of the storm’s past history can be calculated allowing for a more accu- 
rate prediction of the storm's future track. There Vac NCO standard deviationdor obscim as 
wind data. If JI WC can obtain standard deviations ог оросо сс w India iie GC 
be used. The wind data obtained has much missing data, some times causing an unsta- 


ble uten 
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APPENDIX A. STORM.FOR 


This is a listing of the STORM.FOR program used to generate the data for the 
target tarcks presented in this thesis. In order to run this program, the STORMI.DAT 
or STORM2.DAT file must be available. 


С же STORMI Y vz 

Сетот TO RUN МИТА ИУ Лг: 

C 

C т) БК5СКЕ STORM DATA IS AVAILABLE 

C ИО БТОГМІ ОК $10812 

C 3) COPY OESDATA,FILDATA, & SMDATA --» MATLAB SUB-DIR. 

б 4) BEGIN MATLAB --> RUN STORN?2. M 

C 

Ее 
WAN TITIS PROGRAM EMPLOYS AN ADAPTIVE EXTENDED KALMAN 

MEE oii Wilh A FIXED INTERVAL SMOOTHING ALGORITHM TO TRACK A 

C M d el Кар е ШІ, LA TERDEE e т шшр. 

С —— venne Yee ye Te NET en 49 pr vs 

С VARIABLE DEFINITIONS =: 

C AK - SMOOTHING FILTER GAIN MATRIX 

C AKT = TRANSPOSE OF AK 

C RRG = MEASURED TARGET BEARING IN RADIANS 

C BRAANL = PREDICTED TARGET BEARING MEASUREMENT 
б IN RADIANS BRG(K|K-1) 

C DBRG = MEASURED TARGET BEARING IN DEGREES 

© [p = lI P ЛУ BEIWEEN OPSERVATIOANS, T(K) 
Е - ТСК) 

G DTOR = DEGREE TO RADIAN CONVERSION FACTOR 

C Би 2 = MEASUREMENT RESIDUAL, Z(CK) - H(X(K|KR-1)) 
C ІІІ Е2М1 = MEASUBRENBNI RESIDUAL AT PREVIOCS 

C OBSERVATION 

C ESI? E212 = MEASUREMENT RESIDUAL TWO OBSERVATIONS 
C PREVIOUS 

C FACI = RECIPROCAL OF VARE 

C G = KALMAN GAIN VECTOR 

C GATEI = 1. 5*STANDARD DEVIATION OF RESIDUAL 

C PROCPSS USED AS A САТЕ ІХ 

C MANEUVER DETECTION 

C H = MEASURE TEDI MATIRIA 

C EDG = ESTIMATED TARGET HEADING № DEGREES 

C HT = ОП 

C j = COUNTER 

C IMAT = 4 XN & IDENTITY MATRIA 


сео ооо ос ооо ооо с ооо оо оо ооо о ос оо ооо e 


са 


Ј = COUNTER 

K = ITERATION INTERVAL 

LPKK = STATE COVARIANCE MATRIX AFTER PREVIOUS 
OBSERVATIONS 

ТРККМ1 = A PRIORI STATE COVARIANCE ESTIMATE 

LXKK = STATE ESTIMATE AFTER PREVIOUS OBSERVATIONS 

LXKKM1 = A PRIORI STATE ESTIMATE 

M1,M2 = AVERAGE OF RESIDUALS OVER LAST THREE 
OBSERVATIONS 

PHI = DISCRETE-TIME STATE TRANSITION MATRIX 

PHIT = TRANSPOSE OF PHI 

PI = 3. 141592654 

PKK = ESTIMATION ERROR COVARIANCE MATRIX, P(K]K) 

PKKS = SMOOTHED ERROR COVARIANCE MATRIX 

PKKM1 = PREDICTED ESTIMATION ERROR COVARIANCE 
MATRIX, P(K|K-1) 

PKKMIS - PREDICTED ERROR COVARIANCE MATRIX FOR 
SMOOTHING, P(K*1]|K) 

IPKKMIS = INVERSE OF PKKM1S 

PSS = ERROR COVARIANCE MATRIX FOR 
SMOOTHING, P(K{K) 

R = MEASUREMENT NOISE COVARIANCE 

RANGE = DISTANCE FROM SENSOR TO A PRIORI TARGET 
POSITION 

RTOD = RADIAN TO DEGREE CONVERSION FACTOR 

SPD = ESTIMATED TARGET SPEED IN KNOTS 

TEMP = TEMPORARY STORAGE MATRICES USED IN 
MATRIX OPERATIONS 

VARE = VARIANCE OF RESIDUALS PROCESS 

XDIFF Е DISTANCE IN X DIRECTION FROM SENSOR TO 
A PRIORI TARGET POSITION 

XKK = ESTIMATED TARGET STATE VECTOR, Х(К|К) 

XKKS = SMOOTHED TARGET STATE VECTOR 

XKKM1 = PREDICTED TARGET STATE VECTOR, 
ХОК ЕТ) 

ХККМ1$ = PREDICTED TARGET STATE VECTOR FOR 
SMOTHING, X(K+11K) 

XPOS = ESTIMATED TARGET POSITION IN X 
DIRECTION 

XS - SENSOR POSITION IN X DIRECTION 

Xss = TARGET STATE VECTOR FOR SMOOTHING, 
х(кік) 

NG = TRUE TARGET POSITION IN X DIRECTION 

YDIFF = DISTANCE IN Y DIRECTION FROM SENSOR TO 
A PRIORI TARGET POSITION 

YPOS - ESTIMATED TARGET POSITION IN Y DIRECTION 

YS - SENSOR POSITION IN Y DiRECTION 

YT = TRUE TARGET POSITION IN Y DIRECTION 

zs - OBSERVED POSITION IN X DIRECTION 

ZY = OBSERVED POSITION IN Y DIRECTION 


VARIABLE DECLARATIONS 
CEARACTER*1 А,В 


REAL” 
Кра 


NEKK(4,1),XKKM1(4,1),LPKKM1(4,4) , LXKKM1(4, 1) 
H(2,4),HT(4,2),6(4,2),TEMP1(2,1),TEMP2(2,4) , TEMP3(2, i) 


фр 
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e 


TEMP4(4,2) , TEMP5(4,1), TEMP6( 4,4) , TEMP7( 4,4) 

PKK( 4,4) , PKKM1(4,4) ,2(2,1) 
LXKK(4,1),LPKK(4,4),XS(10) , YS( 10) , DBRG( 10) , BRG 
PHI(4,4),PHIT(4,4) , IMAT(4,4) ,XT, YT 

GATE1 ,E(2,1),VARE( 2,2), IVARE( 2,2) 

DT ,DTF,XDIFF , YDIFF ,RANGE ,XS1,YS1,BRG1,BRKKM1 

DATE ,HR,MN, LAT, LONG, TOTIM, TIME ,TIMEM1, DATE1 
OBSERR( 300) , FAC1, SIGTH2 , SIGVT2 ,R( 2,2) , ETCTAL, EAVG, RTOD 
X2,YS2,BRG2,ZX,ZY,M1,E1,E1M1,E1M2,DTOR, TRKERR( 300) 
М2,Е2,Е2М1,Е2М2,611,513,621,023,2ХМ1,2ҮМ1 
XKKS(4,1,300), PKKS(4,4,300) 
XNNM1(4,1),XSS(4,1),XKKM1S(4, 1) 
PNNN1(4,4),PSS(4,4) , PKKM1S(4,4) , IPKKM1S(4,4) 
AK(4,4),AKT(4,4) ,II(4,4) , STRKERR( 300) , DTS( 300) 


х2 TEMP1S(4,4),TEMP2S(4,1), TEMP3S(4, 1) 


TEMP4S8(4,4) ,TEMP5S(4,4) , TEMP6S(4,4) 
AS , ASÀ ,ASL,NAV, MET 


INTEGER" pP 


INTEGER* 


, PCN 


OPEN OUTPUT DATA FILES 
OPEN( UNIT=2 ,FILE='STORM1. DAT’ ,STATUS=' OLD’ ) 
OREN ONIT=3, FILE =' OUTDATA. DAT' ,STATUS= NEW ) 
OPEN(UNIT-4, FILE- TRUDATA. DAT' , STATUS= КЕК") 
GPEN( UNIT=5 , FILE=' FILDATA. DAT , STATUS=" NEW! ) 


OPEN(UNIT=6 , FILE=' 
OPEN(UNIT=7 , FILE=' 


SMDATA. DAT' ,STATUS- “КЕМ” ) 
ELLIPDAT. DAT! , STATUS=' NEW’ ) 


OPEN( UNIT=8 , FILE='MATRIX. DAT' ,STATUS=" NEW’ ) 
OPEN( UNIT=9 , FILE='ERRDATA. DAT' , STATUS=' NEW’ ) 
OPEN( UNIT=10 , FILE=' ERRSDATA. DAT' , STATUS=' NEW’ ) 


RADIAN/ DEGREE 
RTOD-57. 


DTOR=0. 


COMPUTE 4X4 
DO 
DO 
IF 


r Ir 


CONVERSION FACTORS 
2905054193] 
Ол аз 49/3 


ТМАТ(1,Ј)=9.0 


CONTINUE 


БЕ 5-1 


DO 6 2-1,4 
H(I,J)=0. 0 
CONTINUE 


NG I= 
КО = 


PNITIALIZE TIHE 


10 
TD 


COUNTER 


1а-о. 0 
ТІМЕМ1=0. 0 


NP-Ü 


fS 


4442 


INITIALIZ 


COUNTER- FOR 


MANEUVER GATE 


49 


E1M1=0. 0 
Е1/2=0. 0 


C COMPUTE BEARING MEASUREMENT COVARIANCE 
C BEARING ERROR STANDARD DEVIATION = 1 NM 
WRITE(* е“) FILTERING M DATA WITH KALMAN FILTER' 
WRITE(* %) тш 
810 READ(2, 1001, END- 800)DATE, HR,MN,LAT,A,LONG,B,PCN,NAV,MET 
С. БАТЕТЕТ Е АЯА FOR MEASUREMENT NOISE COV. MATRIX VALUES 
ТГОРСМ ВО. Е Е. 
AS=256. 0 
ELSEIF PCN EQT IMMEN 
AS=900. 0 
ELSEIF(BCNSEQ JANTEN 
AS=1600. 0 
C RADAR DATA 
Bion lk (PCN. EQ. 2 HEN 
AS=100. 0 
ЕРЕКЕТТЕР МЕС Дени 
AS=225.0 
ELSEIF( PCN EOS OEE | 
AS=625. 0 
C AIRCRAFT DATA 
ELSE 
А5<((КАУ) «2 + (МЕТ) #2) «0. 5 
ENDIF 
R(1,1)=AS 
I 2500 
R(2,1)=0. 0 
к(2,2)=А5 


\) - 


УРА АЊА АЈА БАЈА БАКА ЈАР воа оле а Пак АЊА АЈА АЊА at. „ә eS nho m e nt oe t... 4, ала вазата с да рала рида аа == ont Vor m e orm prm n rn m m rm e m ~". eas an је 
EVES FV EV AN EM ED ED ES ES FY ER EN EU EL SE GR FV CR FV ER FR GH FV GREY’ 4S HE EY Ж о оъ оъ л ол Фу ет Фу 4% Ж 4% % “éA EC ES EU EN EV AL EL ERED FI EN CU EL EV CV ED 


C READ IN OBSERVATION PACKET (DATE IME UA pn P O) 
C DISTINE IIT) 


C READ(2,1001,END-800)DATE,HR,MN, LAT, A, LONG, B 
1001 FORMAT(F6.0,F2. 0, F2. 0; F320 ATTESA O Ali S ИО 


NP=NP+1 


MN=MN/60. 0 

LAT=LAT/ 10 

LONG=LONG/ 10 

TIME=HR+MN 
C WRITE (3,1) DATE,HR,MN,LAT,A,LONG,B 
FORMATCIN,E7.0,4X,E3.0) 188 B0 120000 EIU DENIS, 


=. 


Tre aba UDIN 
DATE1=DATE 
dE S 

Е ии 


Сл 


No) 


602 


C 


ПЕ DATE HE. DATEL) THEN 
TIME=TIME+24 
ӘІ-ТІМЕ-ТІМЕМІ 
ПАЛЕ ТИМЕ - 24 

ELSE 
DIETIME-TIMEMI 

ENDIF 


DTF=DT*60. 0 

DTS(NP)=DT 

TOLLI- TOTTINIDT 

WRITE (3,2) TIME,TOTTIM,DT 
FORMAT(1X,F7.4,5X,F6. 2, 5X, F6. 2) 


БӘР ЕІКӘРИТСЕРЕТ, ВТ) 


Z(1,1)=LONG 
Z(2,1)=LAT 
TONE 
ZY=LAT 


IF(NP. EQ. 1) THEN 
CALL INIT( LONG, LAT,XKK, PKK) 
ТЕ що оро 
DO 601 I=1,4 
LXRK(I,1)=XKK(I,1), 


WRITES, DE САУТ | 
RITE(3,*) (ХЕК(Т, D. [= 1 AD 
E 
МЕЕ) РОО О О 
DOE ОЕА 
д 
l I J FECL) 


МРІШЕ(Э9 2401УРЕК(І.7) 
FORMAT(4F14. 4) 
CONTINUE 


E DIF 


C PROJECT AEEAD STATE AND ERROR COVARIANCE ESTIMATES 





СРЕГ ОКК) 
CALL MATMUL( PHI ,XKK,4,4,1,XKKM1) 


КЕПТЕ = г жу! 57 
ПО 605 I-21,4 
ВЕРЕС. (XERMI(T, 1); Ша 1 ,4) 
ҮЛЕШШІЕТ( 3,57) DE A d ОИ 
ХККЧ1(1,1)=ХЕКМ1(1,1). 

CONTINUE 


(C STI ILA ТТМЕ Оу 


P(K+i/K) = (PHI * PCK]K) * PHIT) + Q 


сл 
eds 


CALL MATRAN( PHI,PHIT,4,4) 
CALL MATMUL( PHI ,PKK.4,4,4,TEMP6) 
CALL MATMUL(TEMP6, PHIT,4,4,4,TEMP7) 
CALL GETQ(Q) 
CALL MATADD(TEMP7,Q,4,4,1,PKKM1) 
DO 408 I=1,4 
DO 408 J=1,4 

LPKKM1( I, J)=PKKM1(1,J) 
CONTINUE 


КЕШЕ РС „ТЕЧЕ “К^ ,ТЇМЕМТ ОЕ 
DO 604 I=1,4 
WRITE(3,402)(PKKH1(1,J) ,J=1,4) 
FORMAT(4F14. 4) 

CONTINUE 


CONTINUE 


C COMPUTE OBSERVATION RESIDUAL 


C 


Е-2(К)-Н“Х(К|К-1) 


CALL MATMUL(H,XKKM1,2,4,1,TEMP1) 
CALL MATSUB(Z,TEMP1,2,1,E) 


C COMPUTE VARIANCE OF RESIDUALS SEQUENCE 
C AND ADAPTIVE GATE VALUE 


C 


УАҚ(ЕУ)-НФРКЕМІЗНТЕЕ 


CALL MATRAN(H,HT, 2,4) 

CALL MATMUL(H,PKKM1,2,4,4,TEMP2) 

CALL MATMUL(TEMP2 ,HT,2,4, 2, TEMP3) 

CALL MATADD(TE!P3,R,2,2, 1, VARE) 
WRITE(3,*)'VARIANCE OF RESIDUALS = ',VARE 
GATE1=1. 5*SQRT( VARE) 


C COMPUTE KALMAN GAIN MATRIX 


Саса са СО 


G-PKRKMI*HT*(H*PRRMI*HTTR)* 5-1 


CALL MATRAN(H,ET,2., 4) 
CALL MATMUL(PKKM1,HT,4,4,2, TEMPA) 
CALL MATINV(VARE,2,IVARE) 

CALL MATMUL( TEM?4 , IVARE,4,2,2,G) 


WRITE(3,*)'PKKMI*HT z' 
ПИЕ 
WRITE(3,*)TEMPA(I,1) 


CONTINUE 

CREES CN 

DO 613 I=1,4 

WRITE(3,*)G(I,1) 

CONTINUE 

пре бин оо TEEN 
eldest a 
G23=6( 3,1) 


ELSE 


tay 
ој 


БЕСІГІ 1) 
622-603. 1) 
ENDIF 


OOO 


C COMPUTE UPDATED ESTIMATE 

C X(K|K)=X(K|K-1)+G*E, WHERE E=Z(K)-H*X(K|K-1) 
CALL MATMUL(G,E,4,2,1,TEMP5) 
CALL MATADD(TEMP5,XKKM1,4,1, 1, XKK) 


C ЕЕЗ) ХОЛМЕ, Де апаратна 
ШІШЕй5 1-14 

C URTTEC(3, 7 )XKKCI,1) 

605 CONTINUE 


C COMPUTE UPDATED ERROR COVARIANCE MATRIX 

Ç Еро CH PKK) 
CALL MATMUL(G,H,4,2,4, TEMP6) 
CALL MATSUB( IMAT,TEMP6,,4 ,4 , TEMP7) 
CALL MATMUL(TEMP7 , PKKM1,4,4,4, PKK) 


C WRITE GEN) PO UIE, | TINE Б. 3): 
ПО сбощ 1, 4 

С WRITE(3,406)(PKK(1,J),J=1,4) 

406 FORMAT(4F14. 4) 

606 CONTINUE 


C THESE STATEMENTS ARE FOR THE SMOOTHING ALGORITHM 


DO 620 1-1,4 
XKASCI,1,NP)=XKKCI,1) 
620 CONTINUE 


DO 650 I-1,4 
DO 630 J=1,4 
PRKKS(1,J,NP)=PKKC I,J) 
630 CONTINUE 


C COMPUTE TRUE TRACKING ERROR 
ASA-XKK(1, 1) 
ASL=XKK(3, 1) 
TRKERR( NP )=SQRT( ( LAT-ASA) ****2+( LONG-ASL) #2) 


C COMPUTE OBSERVATION ERROR 
C OBSERR(NP)ZSQRT( CASLAT -2X)***24( ASLONG-ZY )*70*2) 


C SAVE LATEST RESIDUALS FOR AVERAGING 
C E1-E 


| COMPUTE THE AVERAGE RESIDUAL OVER THE PAST THREE OBSERVATIONS 
о ЕР Е иа Ма) ја 


WRITE(,*)'PAST THREE RESIDUALS FOR SENSOR 1 ARE : ',EL,E1MI,E1M2 
WRITE(*.*)' BEARING AVERAGE OF SENSOR 1 = '.М1 
WRITE(*,%") ‘MANEUVER GATE FOR SENSOR 1 = | ,GATE1 


сака с? 


C EIMZETMI 


C Е1М1<Е1 
С СОМРСШЕ ERO ELEIPSE ПАША 
С CALL БЪЛТРСХККСТ, 1), ХККСЗ, Ду, КАС TT Е 


C COMPUTE ESTIMATED X-Y POSITION, COURSE, AND SPEED 
| XPOS=XKK(1,1) 
YPOS=XKK(3,1) 
IF (XKK(2,1).EQ.0 . AND. XKK(4,1).EQ.0) THEN 
HDG=0. 0 
ELSE 
HDG=RTOD**ATAN2( XKK(2,1),XKK(4,1)) 
ENDIF 
IF (HDG. LT. 0.0) HDG=HDG+360 
SPD=60%*SQRT(XKK(2,1)##2+XKK(4,1)##2) 


Е WRITE(*,**) 'FILTERED DATA FOR DATA POINT',NP 
WRITE(3,*) 'FILTERED DATA FOR DATA POINT' ,NP 

© WRITE(*,*) 'TIME X POS Y POS HEADING SPEED' 
WRITE(3,*) 'TIME X POS Y POS HEADING SPEED' 

C WRITE(* ,**)TOTTIM, XPOS, YPOS,HDG,SPD 


WRITE(3 ,**) TOTTIM, XPOS, YPOS , HDG, SPD 

WRITE(4 ,*)TOTTIM, ZX, ZY 

WRITE(5 ,**) TOTTIM, XPOS, YPOS, PRK( 1, 1) 

WRITE(9,%*)NP, TRKERR(NP) 
1002 FORMAT( 1X, 5F10. 3) 
1003 FO АТО Po. 2, 3k ЕПО. 1, 25 ви. зе 35 РВ. 1) 
1004 FOR ЕСО ЕО) 


C COMPARE BEARING ERRORS TO MANEUVER DETECTION GATES 


TE ((АВ5(М1). GT. (GATE1))) THEN 


WRITE (7,76) | 2" MANEUVER DETECTION **% 
C WRITE(3,° +) ‘vere’ MANEUVER DETECTION et 
CALL REINIT(DT,ZX, ZY,ZXM1,Z2YM1,LPKKM1,XKKM1, PKKM1) 
Е1:11=0.0 
у= о 
GOTO 205 
EDIE 


DEBE I= Die 
DATE1=DATE 


2541-25 
AHI 


GOI SO 


С ІНІБ І5 ҮШЕБЕ ТІБ ӘМЕОТИТАБ АБО o е 

C FIXED INTERVAL SAHOOTHING 

800 КЕТТЕТ, з) ‘SMOOTHING FILTERED DATA WITH A’ 
WRITEC*,*) E ый EE SMOOTHING ALGORITHM' 

WRITEC* Я zi ) Seite У 


DO 1000 KK=1,NP-1 
K=NP-KK 


DT=DTS(K+1) 


MEST TMENI-DT 
ALL FINDPHICPHI, DT) 


DO 901 I=1,4 
Bies(1,1)=%KKS(1,1,K) 
901 CONTINUE 
DO 902 I-1,4 
DO 902 J=1,4 
POSIT PERS JK) 
902 CONTINUE 


C CALCULATE THE PREDICTED STATE AND ERROR COVARIANCE MATRICES 
Е X(K+1]K)=PHI*X(K[K) 

CALL MATMUL (PHI,XSS,4,4,1,XKKM1S) 
C Р(К+1 | К)ЕРНТ“Р(К|К)“РНТТ+О 

CALL MATRAN (PHI,PHIT,4,4) 

CALL MATMUL(PHI,PSS,4,4,4, TEMP6) 

CALL MATMUL(TEMP6 ,PHIT,4 ,4,4 , TEMP7) 

CALL GETQ(Q) 

CALL MATADD(TEMP7 ,Q,4,4,1,PKKM1S) 


C CALCULATE THE SMOOTHING FILTER GAIN MATRIX 
С AXK=P(K|K)**PHIT*INV°P(K+1[K) 
CALL MATINV (PKKM1S,4, IPKKM1S) 
CALL MATMUL (PKKM1S,IPKKM1S,4,4,4,II) 
CALL MATMUL (PSS,PHIT,4,4.4,TEMP1S) 
CALL MATNUL (TEMP1S,IPKKM1S,4,4,4,AK) 


DO 904 [=1,4 
ХЫММ1(1,1)-ХКК5(1,1,К-Ғ1) 
904 CONTINUE 


C CALCULATE THE SMOOTHED STATE ESTIMATE 

C XKKSEX(K|] K)FAK*(X(K*1|N) -X(K*1]|K) 
CALL NATSUB (XNNM1,XKKM1S,4,1,TEMP2S) 
CALL MATMUL (AK,TEMP2S,4,4,1, TEMP3S) 
CALL MATADD (XSS,TEMP3S,4,1,K,XKKS) 


DO 906 151,4 
DO 906 J=1,4 
РАНТЕ Ј)ЕРККО(Т,Ј,К-Ј) 
906 В МС 


C CALCULATE THE SMCOTHED COVARIANCE MATRIX 
б PRKS=P(K/K)+AK*{ P(KEL|N)-P(K+1|K)] *AKT 
CALL MATSUB (PNN'1,PKEMIS,4,4,TEMP4S) 
1 RAN (АК АКТ, А) 
CALL MATMUL (АХ, ТЕМРАЄ, 4,4,4 ,ТЕМР58) 


UA 
mA 


CALL MATMUL (TEMP5S,AKT,4,4,4, TEMP6S) 
CALL MATADD (PSS,TEMP6S,4,4,K, PKKS) 


C COMPUTE ESTIMATED X-Y POSITION, COURSE АКБ БЕ ЕЕ 

SXPOS=XKKS(1,1,K) 

SYPOS-XKKS(3,1,K) 

IF CXKRSC2,1,K).EQ. 0 AND. -XKNSCA, 1S KOGUEO SO EROS 
SHDG=0. 0 

ELSE 
SHDG=RTOD*ATAN2( XKKS(2,1,K) ,XKKS(4,1,K)) 

ENDIF 

IF (SHDG. LT.0.0) SHDG-SHDGT360 

SSPD=60*SQRT( XKKS(2,1,K)**2+KKKS(4,1,K)**2) 


С WRITE(*,*) 'SMOOTHED DATA FOR DATA POINT' ,K 
WRITE(3,*) 'SMOOTHED DATA FOR DATA POINT' ,K 

(5 WRITE(*,*) 'TIME X POS Y POS HEADING SPEED' 
WRITE(3,*) 'TIME X POS Y POS HEADING SPEED' 

S WRITE(*,*)TOTTIM,SXPOS , SYPOS, SHDG,SSPD 


WRITE(3,*)TOTTIM,SXPOS,SYPOS,SHDG,SSPD 
1010 FORMAT( 1X,5F10. 3) 
1020 FORMAT(IX,F6.2,3X,F10. 1,2240] 000900090 ЖО 
1030 FORMAT( 1X,F6. 2, 3(F8. 1,2X)) 


TINEMI-TIME 
1000 CONTINUE 


C CLOSE(UNIT-24) 


C CALCULATE THE SMOOTHED TRACKING ERROR 
С OPENCUNIT=4. FILE='TRUDATA: БАТ’ STANU: = О 
DO Е.Р 
о“ РОЗЕ 1 I) 
SYPOS=XKXS(3,1,K) 
6 READ(4,1001)DATE,HR,MN,LAT,A,LONG,B, PCN 
STRKERR(K)=SQRT( ( LAT-SXPOS )****2+( LONG-SYPOS )****2) 
WRITE(6,1120)K SXP0S SSPO par GI DO 


ARITE TOP JIE STRRERRCR] 
1100 CONTINUE 
1110 FORMAT( 14, 2F8. 1) 
ilg FOR LATA, DN) 
1139 FORHATCIS SES. 1) 


CLOSE(UNIT-22) 
CLOSE( UNIT=3) 
OEC n ПА. 
CLOSE(UNIT=5) 
CLOSE(UNIT=6) 
Во Ш.) 
CLOSE( UNIT=8) 
Co о) 
CLOSEC Ut =10) 


WRITE(*,*) 'FILTERED & SMOOTHED OUTPUT DATA IS LOCATED IN THE’ 
WRITE(*,%*) 'DATA FILE OUTDATA. DAT. FOR GRAPHIC RESULTS, ' 
WRITE(*,*) 'ENSURE OBSDATAÁ. DAT, FILDATA. DAT, & SMDATA. DAT ARE’ 


сл 
mS 
- 


WRITE(*,*) ‘IN THE MATLAB SUB-DIRECTORY AND RUN THE MATLAB' 
WRITE(*,**) 'M-FILE STORM2. M' 
STOP 
END 


(Cewe NTEN NTEN NT NT TEN TE NTT NTT тосе еее 


И SUBROUTINES 


CO WAT WI WI Y E E VEI T Моји о Sues edn acne m aat ا‎ Lb s'a Ут => vz 29 = ~ a = - - ae ут. .-. “ma = 
еж. 2527 о УУ УУ ТУ И УУ И ЛИЛИ НИЛИ ИУ ЕУ 


СЗ С) 


ALL ELI HH M 


С vU e gee Tee vede eyed EE 
C COMPUTES THE VALUES OF THE PHI . 
С Yvoty mS VR отечена вода возе доде моиве чен Е ая 


REAL™4 PHI(4, DT 


DO 1501 I21,4 
DO. 150859. 4 
DO 1501 K=1,2 
РНІ(І,Ј)=0.0 
1501 CONTINUE 


C COMPUTE PHI MATRIX 
DO 1500 I=1,4 
РНІ(І,1)=1.0 
1500 СОХТІҚСЕ 
PHI(1,2)=DT 
F| PDT 


RETURN 


END 


a ЧЕ Ec LE bo eae 
THIS ROU TINE INITIALIZES THE STATE 

. б o E PS И 

“REAL 4 RECO. 1), РЕКА, B 

REAL*4 L£] , LONG 


ия 


са СЗ Су СЗ 


C INITIAL STATE ESTIMATE 
ШИЕ ЗІ IAT 
0210080 
ЕШ 1)-1056 
02219-00 


C INITIAL ERROR COVARIANCE ESTIMATE 
PRC 1, 1)=100. 0 
PKK(1,2)=0. 0 
PXK(1,3)=0. 0 
PKK(1,4}=0. 0 
Pie 2. 1)=0.0 


= 


РИ (2 2)=0.025 


J l > 
РКК(2,4) 
БІН Г) 
БЕРЕД 
PREG) 
РКЕ Зен) 
о 1 

E42) 
ОО) 
PKK(4,4)= 


l H H H H H H H H 


TTT-T- T-T T-T- 
СО? 
ко сл с с го оо © Gre 


25 
RETURN 
END 


medi. D 


als ate “5 e's - ~: aalsa su было, 25: sc +. ~ * 9 al, m т - г ~ у "S ta ale - ales LM ala Ји t ще - 
С 5 2575 uae E o 2522757 = eese eyes e js eue ха КТП 4% se 
T 
C ROUTINE To cer 4 MATRIX 
С ,92%21:52 723230 ТТТТ ОТ 22572002 3: v» О РА АУС ар a TE ex ris 


REAL*4 Q(4,4) 


DO 100 I=1,4 
DO 100 J=1,4 
Ша Q(I,J)20.0 
DO 200 I=1,4 
200 Е.О 


КЕТОКХ 


END 


а т n PETA E б о, ‚о ‚РЕ ОШ) 


“..,...... е o's “2 un a n nn nm s m an an an an se se we ae ee er аа 26 auta ~ ا‎ ee „„ ur LIES TS 25 
еъ еу ` 47% 94% Фъ Фу су Фм 4. "V ev S TN 


THIS ROUTINE RE- INIT IALIZES ІШЕ STATE AND ERROR 
ES ee El 


20022722 


REAL*4 DT XERMAYCA, 1), PKKM1(4,4) 
REAL*4 ZX,ZY,2X41,2YMl,LPERHI(À, 4) 


TPT M DIST 
VOLE Ерани 


XKKM1(1,1)=ZX 
XKKM1(2,1)=XDIFF/DT 
XKKM1(3,1)=ZY 
XKKH1(4,1)=YDIFF/DT 


C WRITE(3,*)'REINITIALIZED STATES ARE: 
DO 100 I= 4 i 

Е WRITE(3,*)XKKM1(1,1) 

100 CONTINUE 


PKK211(1,1)=2. 25*LPKKM1(l, 1) 
n 21-00 
М1(1,33-2,25%ІРЫҚЧ1(1,3) 


аман ааа дата тг 4. 4-2 2242422 staatse s'a 242 mm" ~. sles ' sola non ant a mela u риц ра ра ра рад раци рад ращ оба рад рад ращ ри да 
€8 48 Фу «ъ «ъ Фу 48 6» 4. 7. 2747Ж ER FUEL ER ED ER ER ERTL FR Ed. FR FR GR EU FL FR EN FR ERB Ed FB 


OOO 


PKKM1(1,4)=0. 0 
PKKM1(2,1)=0. 0 
РККМ1(2,2)-0. 1111 
РККМ1(2,3)-0.0 
PKKM1(2,4)=0. 0 
PKKM1(3,1)22. 25*LPKKM1(3, 1) 
PKKM1(3,2)=0. 0 
PKKM1(3,3)22. 25*LPKKM1(3,3) 
PKKM1(3,4)=0. 0 
PKKM1(4,1)=0. 0 
PKKM1(4,2)=0. 0 
PKKM1(4,3)=0. 0 
PKKM1(4,4)=0. 1111 

RETURN 

END 


оос ы К ЗАК Els о es 125 
oce eee Mea s E О а О а ан ас 
THIS ROUTINE COMPUTES THE ESTIMATED 
= P Ко р PERN WA АНЕ 
852022522 72007000055 5 уа лала aa о А 
REALS 4 2х. ZY 
REAL*4 ES ПО Уа БА НЕ 
REAL*4 NUMER ,DENOM 


INITIAL STATE ESTIMATE 


NUMER=( -YS2*TAN(C BRG2) )+C YS1*TAN( BRG1))+XS2-XS1 
DENOMN=TAN( BRG1) -TAN(C BRG2) 


ZY=NUMER/DENOM 
ZX=(ZY~YS1)*TANCBRG1)+XS1 


RETORN 


END 


p IINE E ҮТ, Pl Wc ا‎ 


THIS “SUBROUTINE. COMPUTES. ERROR ELLIPSE DATA. 
d E а nS 
E IOI 
DIMENSIONS AND DECLARATIONS 
meee SUD VT XPC21), YPC21),ÀA, B; IHETISSI602X, SIG2Y 
REAL*4 SX,SY,PT,CT,ST,P1,P15,P3 


Ру Татты ралы! 


DARLES SEU 
uwa а. пъ за. ай = = пови ева = 
Cer TY TNT Vv V NS 


OOO OO 


A=2*P13 
B=P1-P3 

THE1=0. 5*ATAN2 (A,B) 
A=(P1+P3)/2 

30. 0 

Пай ела го 0.0) GOTO 10 


B-P13/SIN(2. O*THE1) 
10 SIG2X-ABS(A-*B) 
5ІС2Ү-АВ5( А-В) 
5Х-5162Х7%0. 5 
5Ү-5152Ү%%0, 5 
PT=3. 141592654/10 
CT=COS( THE1) 
ST=SIN(THE1) 


DOMIDOCIE-IS 21 
XPC IE)=SX*COS( PT*IE)*CT-SY*SIN( PT*IE )*ST+XT 
YPCIE)=SX*COS( PI*IE)*SI+SY*SIN( Plea 
CRITE 7 SPCTEJGHARCO s Yi ШЕ) 
100 CONTINUE 
RETURN 


END 


Шы кор Ва р, Е D M 


C Ue 259722509 олак оо онор ш: 
G THIS ROUTINE MULTIPLIES TWO 12. TOGETHER 
(2 Р ae DN - e m * т об ре 
С зи ae ок aa erence ee 
C DIMENSIONS AND DECLARATIONS: 
EET OTT BEND CEN) 
DO 10 I-1,L 
DO 10 J=1,N 
CCI,J)=0. 0 
10 COSTI CE 
DO 100 I= 1,L 
DO 100 J= 1N 
DJT LOOC k= IN 
С(І.Ј) = BCI-J тв 
100 CONTINUE 
RETURN 
END 


Ro p ^N o.‏ ك 


в: о о оо ра 


С ии 552755525. 

e THIS ROUTINE TRANSPOSES А MATRIX 

С : бі МЕ D m D 

С жи мама У У УУ -: 

C Dp (ENSIONS AND DECLARATIONS 
КАБ), ВО? 


ПОО = 21, 
ВОО =, 
БАЈА) = АСТАЈ) 
100 CUST TRUE 


RETURN 


END 


ЕЕ a Ay Nee) 


С eyes eee pc ede gede eyes eee se =. И ее КУ 


C 
C 


- 


THIS ROUTINE MULTIPLIES A MATRIX WITH A SCALAR 
: E e. = и * ACN,M) 


CA Yeeveyesevevevevevev eve ev eee eee eee ee eee eee dede dee dedey eee de devede eee ede eee 


C 


100 


ps 


слее са со 


100 


O C2 C2 C2 C2 


100 


“..... 
40% 4% “” 


mmm prn m RM 
Фу Фъ съ въ #4 7. 


“DIMENSIONS AND DECLARATIONS 
REAL*4 A(N,M), CCN,M), 0 


DO 100 I 
DO 100 J T 

CCI,J) = Q*AC I,J) 
CONTINUE 


ISN 
PS 


RETURN 


END 


SUBROUTINE БОМ Be M ШЕШ) 
POPES TERE TET ETE TET ETE TE TEES TE TSG Пе еее 
THIS ROUTINE SUBTRACTS TWO MATRICES 
қ oo и = E Ee (N 1) 
кутиите и А 
DINENSIONS AND DECLARATIONS 
REAL*4 ACN,M),BCN,M),CCN,M) 


ШШ 071 1,N 

БӘЙ Jos 1,5 
С(І,Ј)=А(І, Ју. ECTS) 

CONTINUE 


RETURN 
END 


п NE pee CAB 
erede и vc 25724789952 о 
THIS ROUTINE “ADDS TWO MATRICES 
: ва | = а ОА $ eo а 
DIMENSIONS: AND DECLARATIONS. 
ГЕИ АО М). ВОМ, мМ), СЕМ: В) 
DO 100 I = 1,۸ 
poo = 1,М 
GUI... L)eA(CI. Sere Cl.) 
CONTINUE 


RETURN 
END 


61 


EON СА, M M‏ ا 


ale alo alo 


Qv IUe aa 2. АС Сао 
e THIS ROUTINE. COMPUTES: p INVERSE OF 
C A MATRIX 
C Er N)7INV [ACN,N)] 
Qe ees KA a a eeu ME ero dre Pepe ОСУУСУ ОСС СОС ЗСУ ЕЕЕ 
C DIMENSIONS AND DECLARATIONS 
REAL AND COT DCD) 
Ны 
DO 100 ее 
100 БОЛ КУА ЛО) 
DO 115 1515 
DO 115 JzN-*1,2*N 
115 D ода 
DO 120 I=1,N 
J=I+N 
120 П 
DO 240 K=1,N 
M=K+1 
ТЕ (КОЕ КУ GOTO 150 
L=K 
DO 140 I=M,N 
140 IF (ABS(D(1,K)).GT. ABS(D(L,K))) [el 
ТЕ CDSBOSPRUSGOITO 189 
DO 160 J-K,2*N 
TEMP=#D(K, J) 
ме) 
160 DCE Joss psp 
180 РОА О ин Јен 2, 
185 DET INGE 
IE (KEQ TI" GOTO 220 
H1=K-1 
ВО 200 1111 
DO 200 J-2M,2*N 
200 БЕТ „Јус ВС 4) “РС 1 EDT 
ТЕК ЕОМ) COTO 260 
220 DO 240 I=M,N 
DO 240 J2M,2*N 
240 КЕЛЕКЕ, ND 
260 DO 265 I=1,N 
ВОО ЕЁ. 
K9JTN 
265 Өсе PCI M 


RETURA 
ЕД 


APPENDIX B. WIND.FOR 


Ihis a lisung of the WIND.FOR micro computer program used to generate the data 


for the storm wind speed tracks presented in this thesis. In order to run this program. 
the WINDOI.DAT file must be available. 


C 


oo 


со о ооо о оо оо ес ооо OP OP CO еее ео 


еее 


.“... 


АК 

АКТ 
BRG 
BRKKM1 


DBRG 

DT 

DTOR 
E1,E2 
Е1М1,Е2М1 
E1M2,E2M2 
РАС] 

С 

САТЕ1 


H 

HDG 
HT 

I 

IMAT 

J 

K 

LPKK 
ТРККМ1 
LXKK 
LXKKW1 
M1,M2 
PHI 
PHIT 
PI 

PKK 
PKKS 
р 
PKKMIS 
IPKKM1S 
PSS 
RANGE 
RTOD 
SPD 
TEMP 


зе“ МАКТАВЪЕ DEFINITIONS**- 


I H H ll H H H H H 


I H H H lH H H d l H H H ll H H l l H H HT H H H H H H IH 


SMOOTHING FILTER GAIN MATRIX 

TRANSPOSE OF AK 

MEASURED TARGET BEARING IN RADIANS 

PREDICTED TARGET BEARING MEASUREMENT IN RADIANS 
BRG(K|K- 1) 

MEASURED TARGET BEARING IN DEGREES 

TIME DELAY BETWEEN OBSERVATIONS,T(K) - T(K1) 

DEGREE TO RADIAN CONVERSION FACTOR 

MEASUREMENT RESIDUAL, Z(K) - H(X(K|K-1)) 

MEASUREMENT RESIDUAL AT PREVIOUS OBSERVATION 

MEASUREMENT RESIDUAL TWO OBSERVATIONS PREVIOUS 

RECIPROCAL OF VARE 

KALMAN GAIN VECTOR 

1. S*STANDARD DEVIATION OF RESIDUAL PROCESS, USED AS / 
GATE IN MANEUVER DETECTION 

MEASUREMENT MATRIX 

ESTIMATED TARGET HEADING IN DEGREES 

TRANSPOSE OF H 

COUNTER 

4 X 4 IDENTITY MATRIX 

COUNTER 

ITERATION INTERVAL 

STATE COVARIANCE MATRIX AFTER PREVIOUS OBSERVATIONS 

A PRIORI STATE COVARIANCE ESTIMATE 

STATE ESTIMATE AFTER PREVIOUS OBSERVATIONS 

A PRIORI STATE ESTIMATE 

AVERAGE OF RESIDUALS OVER LAST THREE OBSERVATIONS 

DISCRETE-TIME STATE TRANSITION MATRIX 

TRANSPOSE OF PHI 

3. 141592654 

ESTIMATION ERROR COVARIANCE MATRIX, P(K|K) 

SMOOTHED ERROR COVARIANCE MATRIX 

PREDICTED ESTIMATION ERROR COVARIANCE MATRIX, P(K|K-1 

PREDICTED ERROR COVARIANCE MATRIX FOR SMOOTHING, P(K+ 

INVERSE OF PKKN1S 

ERROR COVARIANCE MATRIX FOR SMOOTHING, P(K 

MEASUREMENT NOISE COVARIANZE 

DISTANCE FROM SENSOR TO A PRIORI TARGET POSITION 

RADIAN TO DEGREE CONVERSION FACTOR 

ESTIMATED TARGET SPEED IN KNOTS 

TEMPORARY STORAGE MATRICES USED IN MATRIX 





K) 





C OPERATIONS 

C VARE = VARIANCE OF RESIDUALS PROCESS 

C XDIFF = DISTANCE IN X DIRECTION FROM SENSOR TO A PRIORI 
C TARGET POSITION | 
C XKK = ESTIMATED "TARCE ME TATE VECTORS XE 

C XKKS = SMOOTHED TARGET STATE VECTOR 

C XKKM1 = PREDICTED TARGET STATE VECTOR, X(K|K-1) | 
C ХККМ15 = PREDICTED TARGET STATE VECTOR FOR 5ММОТНІМС, ХОВ 
C XPOS = ESTIMATED TARGET POSITION IN < DIRECTS | 
e XS = SENSOR POSITION IN X DIRECTION 

C XSS = TARGET STATE VECTOR FOR SMOOTHING, X(K|XK) 

C ANT = TRUE TARGE MPFOSTT TONSIL TA DIRECTION 

C ИВЕТ = DISTANCE IN Y DIRECTION FROM SENSOR TO A PRIORI 
C TARGET POSITION 

C YPOS = ESTIMATED TARGET POSITION INT DIREC 

C YS = SENSOR POSITION IN Y DIRECTION 

C 47 = TRUE TARGET POSITION IN Y DIBESWTION 

C aX = OBSERVED POSITION IN X DIRECTION 

C ди = OBSERVED POSITION IN Y DIRECTION 


C VARIABLE DECLARATIONS 
CHARACTER*1 A,B 


REAL*4 XKK(2,1),XKKM1(2, 1), LPKKM1(2,2) , LXKRM1(2, 1) 

REAL*4 H(2,2),HT(2,2).G(2, 1) , TEMP1(2,1), TEHP2(2,2) , TEMP3(2 , 1) 
REAL*4 TEMPA(2,2),TEMP5(2,1), TEMP6(2,2) , TEMP7(2,2) 

REAL*4 PKK(2,2),PKKM1(2,2),2(1, 1) 

REALS LXKECO 1) DPRROSU2) X9 010) место), DBRG( 10) ВЕЕ 

READ {bul P ШАТО ЛІ ЫШ т 

REAL*4 GATE1,E(2,1),VARE(2,2),IVARE(2,2) 

REAL*4 DT,DTF,XDIFF,YDIFF,RANGE,XS1,YS1,BRG1,BRKKMI 

REAL*4 DATE,HR,MN,LAT,LONG,TOTIM,TIME,TIMEM1 , DATE1 

REAL*4 OBSERR( 300) ,FAC1,SIGTH2,SIGVT2 ,R(2,2),ETOTAL, EAVG,RTOD 
REAL*4 X2,YS2,BRG2,2ZX,ZY,M1,E1,E1M1,E1M2 ,DTOR, TRKERR( 300) 
REAL"4 Мо БЕО БОМБАМА CINCO са ИП 

ЕЕА 4 XIS 1.300) , PRES( Secon) 

REAL*4 XNNM1(2,1),XSS(2,1),XKKM1S(2,1) 

REAL*4 PNNM1(2,2),PSS(2,2) , PKKM18( 2,2) , IPKKM1S(2,2) 

REAL AK(2,2),AKT(2,2), 11(2,2) , STRKERR( 300) , DTS( 300) 

REAL*4 TEMP1S(2,2),TEHP2S(2, 1), TEMP3S(2,1) 

REAL*4 TEMP4S(2,2), TEMP5S(2,2) , TEMP6S(2,2) 


REAL*4 AS,ASA,ASL,WIND,WINDD,NAV,MET 
INTEGER*2 NP,ASIM,K 
INTEGER*, PCN 
C OPEN OUTPUT DATA FILES 
OPEN(UNIT=2 .FILE=WINDOL. DAT: STATUSS OLD) 
OPENCUNIT=3 FILE ="OUIDATA. DAT STATUS NEN) 
OPEN( UNIT=4 , FILE=' OBSDATA. DAT’ , STATUS=' NEW’ ) 
OPEN( UNIT=5 , FILE='FILDATA. DAT' , STATUS=' NEW’ ) 
OPEN( UNIT=6 , FILE='SMDATA. DAT' , STATUS=’ NEW' ) 
OPEN(UNIT=8 ,FILE ='MATRIX. DAT’ STATUS NEN) 
OPEN(UNIT=9 ,FILE ='PALDATA. DAT’ ,STATUS=' NEW’ ) 
C RADIAN/DEGREE CONVERSION FACTORS 
| RRE@DS 57 2057 1051 
DTOR=0. 017453293 
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C COMPUTE 4X4 IDENTITY MATRIX 
DO 5 1=1,2 
У ој] 2 
ЕО JS) THEN 
ТМАТ(1,Ј)=1. 0 
ELSE 
IMAT(I,J)20.0 
ENDIF 
5 CONTINUE 


Н(1,1)=1.0 
H(1,2)20.0 


C INITIALIZE TIME COUNTER 
TOTTIM=0. 0 
TIMEM1=0. 0 
WIND=0. 0 
NP=0 


C INITIALIZE COUNTER FOR MANEUVER GATE 
E1M1=0. 0 
E1M2=0. 0 


» COMPUTE BEARING MEASUREMENT COVARIANCE 
BEARING ERROR STANDARD DEVIATION = 1 NM 
WRITEC* 7) FILTERING ( A DATA WITH KALMAN FILTER' 
WRITE “) е = 
810 READ( 2. 1001, END= 800)DATE, HR ,MN,LAT,A,LONG,B,PCN,WINDD ,NAV МЕТ 
C RADAR DATA FOR MEASUREMENT NOISE COV. MATRIX 
ТЕСОРО , QO. 1) THEN 
AS=100. 0 
ШЕВ O FGD,. EQ. 2 )THEN 
AS-225.0 
ВБ ТЕР. БО. З)ТПЕМ 
А5=625.0 
ELSETE(PCN. CO. 4)THEN 
AS=900. 0 
C AIRCRAFT DATA 
ELSE 
AS=(CNAV)**2+( MET) #*2) 20. 5 
ENDIF 
R(1,1)=AS 
R(1,2)=0.0 
R(2,1)=0.0 
R(2,2)=AS 


СЭ бр 


Fo en au ON ON CS EU OUR ON EN EN ON GR EN OMEN ONES CL ANON ON BN SS OREN ON OP ON OD ON OU OR OR OS OT OR OD ODOR GR ORR OR OR OR ON ON ON OL OD ON OS OL ON OD ON ONO 


© READ IN OBSERVATION PACKET (РАТЕ, ТІМЕ, ТАТ, GONG) 
C PIZ RIECK) TIME E=) 


TOUI Ро. 0,22. 0 Рено О F3. 0,2CP2. 0)) 


401 
602 


MN=MN/60. 0 

LAT=LAT/10 

LONG=LONG/ 10 

TIME=HR+MN 

FORMAT(1X,F7. 0,4X,F3. 0, 2k; FO. Gy 6X ПА, 

NP=NP+1 

IF (NP.EQ.1) THEN 
DATE 1=DATE 
ТІМЕМІ-ТІМЕ 

ENDIF 

IF (DATE. NE. DATE1) THEN 
TIME=TIME+24 
DT=TIME-TIMEM1 
TIME=TIME-24 

ELSE 
DT=TINE-TIMEM1 

ENDIF 


DTF-DT*60.0 

DTS(NP)=DT 

TOTTIM=TOTTIM+DT 

WRITE CF) DT,NP,WINDD 

FORMAT( 1X,F7. 4,5X,F6. 2,5X,F6. 2) 


CALL FINDPEICPHI,DT) 


Z(1,1)-WINDD 
ZY-WINDD 


IF(NP.EQ. 1) THEN 
CALL парен, РКК) 
ОГ 
ро 601 1=1,2 
ЕСІ. 1)-ХҚКС1. m 


M RITE(3,% х) тә Уто; г! 
Пг) (ХЕК(1, 1), So 
CONTINUE 

Е ©) Гита ута! 


WAT pí ОА 


м оо, 
о 

DO 602 J=1,2 

LPKK(I, J)=P КК(І,Ј) 
WRITE(3, 401)PKK(1, J) 
FORMAT( 2F14. 4) 

CO TINUE 


EXDIE 


C PROJECT AHEAD STATE AND ERROR COVARIANCE ESTIMATES 


C 


X(K+1|/K) = PHI * X(K|K) 
CALL MATMUL(PHI,XKR,2,2,1,XKKM1) 
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DO 603 I=1,2 
LXKKM1(1I, 1)=XKKM1(I,1) 
603 CONTINUE 


С P(K-1|K) 2 (PHI * Р(К|К) * РНІТ) + 0 


CALL MATRAN(PHI,PHIT,2,2) 
CALL MATMUL(PHI,PKK,2,2,2,TEMP6) 
CALL MATMUL(TEMP6 ,PHIT,2,2,2, TEMP7) 
UM WECETQUQTDT) 
CALL MATADD(TEMP7,0,2,2,1, PKKM1) 
DO 408 I=1,2 
DO 408 J=1,2 
ТРККМ1(Т, 7) <РККМ1(Г, 7) 
408 CONTINUE 


204 CONTINUE 


C COMPUTE OBSERVATION RESIDUAL 
0 — ESZ(K)-H*X(K|K-1) 
IF(WINDD. EQ. O) THEN 
ELO 
E(2,1)=0.0 
ELSE 
CALL MATMUL(H,XKKM1,2,2,1,TEMP1) 
CALL MATSUB(Z,TEMP1,2,1,E) 
ENDIF 


C COMPUTE VARIANCE OF RESIDUALS SEQUENCE 
C AND ADAPTIVE GATE VALUE 
C VARCE)ZH*PRRNI*HTTR 
CALL MATRANCH,HT,2,2) 
Е HMULCIISPERMT 2:2. 9 TEMP2) 
ПАЛО T i ТЕМРО НТ. 2 2 2 TENPS) 
CALL MATADD(TEMP3,R,2,2,1,VARE) 
C WRITE(3,%) ‘VARIANCE OF RESIDUALS = ' ,VARE 
C GATE1=1. 5*SQRT( VARE) 


C COMPUTE KALMAN GAIN MATRIX 
C G=PRK M1 HT (WPM 1*HT+R ) 2-1 
CALL MATRAN(H,HT,2,2) 
CALL MATNUL(PKKM1,HT,2,2,2,TEMP4) 
CALL MATINV(VARE,2,IVARE) 
CALL MATMUL(TEMP4 , IVARE,2,2,1,6) 


COMPUTE UPDATED ESTIMATE 
X(KIK)=X(K|K-1)+G"E, WHERE E=Z(K)-H*X(K|K-1) 
GAIL NATMULCG,E.2,2,1, TEMPS) 
TL МАТАП (ТЕН 1. XO 


Oc 





6? 


C 


C 
605 


WRITEC3,*) XC’ , TIME, | TIMENS UN 
DO 605 I=1,2 

ПОЕ) КИСТ 1) 

CONTINUE 


C COMPUTE UPDATED ERROR COVARIANCE MATRIX 


C 


P(K]K)=(1 - G*H)*P(K]K-1) 


CALL MATMUL(G,H,2,2,2,TEMP6) 
CALL MATSUB( IMAT, TEMP6,2,2,TEMP7) 
CALL MATMUL(TEMP7 , PKKM1,2,2,2, PKK) 


C THESE STATEMENTS ARE FOR THE SMOOTHING ALGORITHM 


620 


630 


1092 
1003 
1004 


DO 620.I=1,2 
XKKS(I,1,NP)-XKK(I, 1) 

WRITEC*,*)XKKSCI,1,NP),PRKS(I, 1,NP) 
CONTINUE 


DO 630 I=1,2 
DO 630 J=1,2 
PKKS(1,J,NP)=PKK(I,J) 
CONTINUE 


WRITE(3,%*) ‘FILTERED DATA FOR DATA POINT’ ,NP 
WRITE(3,*) ‘TIME VEL. ACCELL. HEADING SPEED’ 
WRITE CSeeo mOTTIMN, XKKC 14 1)4 XKKC2e № 

RETES TOTI IM. ZY 
WRITE(5,%*)TOTTIM,XKK(1 1) XKK ЕК т) 
КЕІТЕ(9,%)ХР 

FORMAT( 1X,5F10. 3) 

FORMAT( 1X. F6. 2, 3X¢F 10. 12x ЕТ ОВИ) 
FORMAT( 1X,F6.2,3(F8. 1,2X)) 


C COMPARE BEARING ERRORS TO MANEUVER DETECTION GATES 


IF ((ABS(M1). GT. (GATE1))) THEN 
WRITE(,7)'** MANEUVER DETECTION 7% ' 
WRITE(3,7*)'** MANEUVER DETECTION “ет! 
САЫ. ЕБІМІТ(рТ,2У,2ҮМІ ТРИКО ТОИР) 
ЕЛ 12070 
Е1М2-0. 0 
GOTO 204 

ENDIF 


TIMEMI=TIME 
DATE 1=DATE 


2ҮМ1-2Ү 
GOTO 810 
WRITECO,* )TOTTIM, XKKGM, 1), CEE ЕЕ НЕ 


C THIS IS WHERE THE SMOOTHING ALGORITHM STARTS 
C FIXED INTERVAL SMOOTHING 


800 


WRITE(*,**) ‘SMOOTHING FILTERED DATA WITH A’ 
WRITE(*,*) ‘FIXED INTERVAL SMOOTHING ALGORITIM' 


OS 


WRITE(* у * 2 Nemen | 


C WRITE (7,*) DT,NP,WINDD 
DO 1000 KK=1,NP-1 

Ç CALL REINIT(DT,ZY,ZYM1,LPKKM1,XKKM1,PKKM1) 
K=NP-KK 


DT=DTS(K+1) 


CIMES: IMENI =DT 
TOTTIN=TOTTIM-DT 
C [5 I DPHICPHI DT) 


DO 901 I=1,2 
MSS( 1, 1)=XKKS(1,1,K) 
901 CONTINUE 


DO 902 I=1,2 
DO 902 J=1,2 
PSS(1,J)=PKKS(1I,J,K) 
902 CONTINUE 


C CALCULATE THE PREDICTED STATE AND ERROR COVARIANCE MATRICES 
C X(K*1|K)2PHI*X(K|K) 

CALL MATMUL (PHI,XSS,2,2,1,XKKM1S) 
C P(K+1|K)=PHI*P(K]K)*PHIT+Q 

CALL MATRAN (PHI,PHIT,2,2) 

CALL MATMUL( PHI ,PSS,2,2,2,TEMP6) 

CALL MATMUL(TEMP6 , PHIT,2,2,2,TEMP7) 

Оние ет оа 

CALL MATADD(TEMP7 ,Q,2,2,1,PKKM1S) 


C CALCULATE THE SMOOTHING FILTER GAIN MATRIX 
C AK=P(K|K)**PHIT*INV® P(K+1 |K) 
CALL MATINV (PKKM1S,2, IPKKM1S) 
CALL MATNUL (PKKM1S, IPKKM1S,2,2,2,11) 
CALL MATNUL (PSS,PHIT,2,2,2,TEMP1S) 
CALL MATMUL (TEMP1S, IPKKM1S,2,2,2,Ak) 


DO 904 I=1,2 
ИО UM -XKNSC(I,1,RT1) 
904 CONTINUE 


C CALCULATE THE SMOOTHED STATE ESTIMATE 

C XKKSEX(K| O*AK*(X(K-1 |N) -X(K*1|K) 
CALL MATSUB (XNNM1,XKKM1S,2, 1, TEMP2S) 
CALL MATMUL (AK,TEMP2S,2,2,1,TEMP3S) 
CALL MATADD (XSS,TEMP3S,2,1,K,XKKS) 


DOSSOOST-I,?2 
DO 906 J=1,2 
БӘӘРГІГІ .J)=PKI88( 1 ,3J%8+1) 
906 CONTINUE 


C CALCULATE THE SMOOTHED COVARIANCE MATRIX 
С PRKSEP(K|K)4AK*[ P(K*1|N) -P(K*1|K)] *AKT 
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1010 
1020 
1030 


1000 


1100 


О 
1120 


Ср СО 


C501 


ae 
rue Lee ле та ав ла es ee el et et ee a eee 


CALL MATSUB (PNNM1,PKKM1S,2,2,TEMP4S) 
CALL MATRAN (AK, AKT,2,2) 

CALL MATMUL (AK,TEMP4S,2,2,2,TEMP5S) 
CALL MATMUL (TEMP5S ,AKT,2,2,2,TEMP6S) 
CALL MATADD (PSS,TEMP6S,2,2,K,PKKS) 


WRITE(3,*) 'SMOOTHED DATA FOR DATÀ POINT',K 
WRITE(3,*) 'TIME VEL. ACCEL. HEADING SPEED' 
WRITE(3,*)TOTTIM, XKKS(1,1,K),XKKS(2,1,K) 

WRITE(6 ,*)TOTTIM, XKKS(1,1,K),XKKS(2,1,K) ,PKKS(1,1,K) 
FORMAT( 1X,5F10. 3) 
FORMAT(1X,F6.2,3X,F10. 1,2X,F11. 1,3X,F8. 1,3X,F8. 1) 
FORMAT( 1X, F6. 2,3(F8. 1,2X)) 


TIMEMI-TIME 
CONTINUE 


CONTINUE 
FORMAT( I4 , 2F8. 1) 
FORMAT(I4,3(F8. 1,2X)) 


CLOSE(UNIZE2) 
CLOSE(UNIT=3) 
CLOSE(UNITz4) 
CLOSE( UNIT=5 ) 
CLOSE(UNIT=6) 
CLOSECUNITS9) 
CLOSE( UNIT=8 ) 
WRITE(**,*) 'FILTERED & SMOOTHED OUTPUT DATA IS LOCATED IN THE! 
WRITEC*,*) "DATA FILE OUTDATA. DAT. FOR GRAPHIC RESULTS, ' 
WRITE(*,*) ‘ENSURE OBSDATA. DAT, FILDATA.DAT, & SMDATA.DAT ARE’ 
WRITE(*,*) 'IN TEE MATLAB SUB-DIRECTORY AND RUN THE MATLAB! 
КЕЛЕСІЗ j pi RO N 

STOP 
END 
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REAL - . PHIC2, 2) Д DT 


DO 1501 I=3,4 
DO 1501 J=1,4 

PHI(I,J)=0.0 
CONTINUE 


C COMPUTES TEATRIN 


DO TOO ISL 


70 


РНІ(1,1)=1. 0 
1500 CONTINUE 
PHI(1,2)=DT 
PHI(2,1)=0. 
PHI(2,3)=0. 
PHI(2,4)=0. 
PHI(1,3)=0. 
PHI(1,4)=0. 


Go G елиса 
Gea |) 


RETURN 


END 


| Шо Е MENS НО), 
THIS ROUTINE INITIALIZES THE STATE 
m a Е. ы ا ا‎ 


OOO CO 


а иа ook becca bes 
REAL 4 XKK(1, 1). PRRC2,2 
REAL*4 WIND, VINDD 


Са 


INITIAL STATE ESTIMATE 
XKK(1,1)=WINDD 
WRITE(*,*) XKK(1, 1) 
XKK(3,1)=0. 0 
ХКК(4,1)-0. 0 


I 


C INITIAL ERROR COVARIANCE ESTIMATE 
РКК(1,1)-1000000. 

PKK(1,2)=0. 
PKK(1,3)=0. 
PKK(1,4)=0. 
PKK(2,1)=0. 
РЕК(2,2)-0. 
PKK(2,3)=0. 
PKK(2,4)=0. 
РКК(3,1)-0. 
РКК(3,2)-0. 
PKK(3,3)=0. 
PKK(3,4)=0. 
PKK(4,1)=0. 
РКК(4,2)-9. 
РКК(4,3)=0. 
РКК(4,4)-0. 


Cael 
съб ав Дар св 2 Со GOONS OOS SS 
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RETURN 
END 
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2 ROUTINE то cer 9 MATRIX 
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сес: 


ее. 


100 


С) С?) 


КЕСА Све Јаска С СС АС C) 


ааа ао ма sles 
ОЪ ОУ УНУ ол е 


DO 100 Iz1,4 
DO 100 J=3,4 

Q(I,J)=0.0 
QC 1,1)2(DT**4)/4. 
CCl a 2 
OA Е) 
ODI) 
DO 200 I=3,4 

DO 200 J=1,4 
ше у= О 


RETURN 


END 


ш ee Y a eae A ARK eRe) 


a-a 2.) eses 
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THIS ROUTINE RE- IN ITIALIZES THE STATE AND ERROR 


ии о 


REAL“ А DT ‚ХЕКМ1(2 E PREMIC2, 2) 
REAL eU ZXMI.AZYMI ,LPKRMI(2, 2) 


XDIFF=ZX-ZXM1 
YDIFFzZY-ZYnl 


NKKM1(1,1)=2X 
XKKM1(1, 1)=ZY 
XKKM1(3,1)=0. 0 
NKKM1(4,1)=0. 0 


WRITE(* Шш. "REINITIALIZED STATES ARE: 


WRIT т 
CONTINUE 


PRTG. 
PKKM1( 1 


1 "РЕК ІН 
2 

PREXITI 3 
Í 


"РАКЕ 1 RED 
PKKM1(1, 


1 


| 


S*LPKKM1(3,1) 
5 ЭПРКЕМИЕЗ 2) 
a 

PER mM 1 

PREM ICG 52 

РККМ1(4,3 

Pre IIIA. 4 


ТЇ Il HW HJ H HW H H H ÓH H H H H Hl H 
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25 
0 
25 
0 
0 
11 
Q 
0 
2 
0 
7» 
0 
0 
0 
0 
1 


DIT 
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tv 
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SUBROUTINE ои 151, KSA I52, ВКС1 ‚ ВВ; ди, таз, 


СО ХИТУ У УУ УУ УУ У У УК УУ nx) сеје ce теу eYeveveveveve m RN ее 
C TITS ROUTINE COMPUTES THE ESTIMATED 
C КА Е n Sri UM BEN p E UNIS 
С vete ии ети И ИИСУСА EOE eS or i Ok bic ik Gi ok aid ay 
REAL" А ZI An 
REAL*4 XS1 151 552, ,YS2 ,BRG]I S bRG2 
REAL*4 NUMER , DENOM 
C INITIAL STATE ESTIMATE 


O O O O CJ 


10 


100 


DIMENSIONS AND DECLARATIONS 


NUMER=( -YS2*TAN( BRG2 ) )+CYS1*TAN( BRG1 ) )+XS2-XS1 
DENOM-TAN( BRG1) - TAN( BRC2) 


ZY=NUMER /DENOM 
ZX-(ZY-YS1)"TAN(BRG1)4XS1 


RETURN 


END 


Bru E A. E. E. па ае 


ап пенса пе пр он toute als steve D sty Mes 
АКА % 


“THIS ‘SUBROUTINE COMPUTES ERROR ELLIPSE DATA 
FROM ERROR COVARIANCE DATA 


Ceo eri ee e ard ov Y Vv YN YN YN YN 


REA XT YT. XPC21) , YPC21), A, B, THET,SIG2X,SIG2Y 
REAL*4 SX,SY,PT,CT,ST,P1,P13,P3 


A=2*P13 

B=P1-P3 

THE1=0. 5*ATAN2(A,B) 
A=(P1+P3)/2 

B=0. 0 

ШОН РИЗ ЕО, 0. 0) GOTO 10 
B-P13/SIN(2. O*THE1) 
SIG2X=ABS( A+B) 
SIG2Y-ABS(A-B) 
SX-SIG2X:3*0.5 
SY=SIG2Y**0. 5 

PT=3. 141592654/10 
CT=COS(THE1) 
ST=SIN(THE1) 


DOF 00 21 
K SD [OS(PISIP)#2OCT-SYVSSINC PT#ZIE)#ST+KT 
j IP) = A COSPI IE) ТЕБУ БТС РТ ТЕ у  CTHYT 
(ҮШ PCIE), CHAR(9), IRLCIE) 

CONTINUE 


RETURN 


]~ 
زرا 


END 


SUBROUTINE МАТИСА ВА АМА O) 
О ЕКИ PELO desde Еу не 
G THIS ROUTINE MULTIPLIES TwO MATRICES TOGETHER 
C SSC Cue N) = . M) * M 
С каки ии ти 2 ааа ао рака аа ана а Пр ОСИЕК 
б DIMENSIONS AND DECLARATIONS 

REAL*4 A(L,M),B(M,N),C(L,N) 


KO па 
DO. 10 J=l N 
C(I,J)=0. 0 

10 CONTINUE 


DO 1O0 T= | 
DO 100 J= 1 
DO 100 K= 1 
СОТ, Л) = С 
100 CONTINUE 


L 
N 
Ë 


— 


(1,7) + ACI,K)*B(K, J) 


RETURN 


END 


ee Uae p В, D ИМ) 
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THIS ROUTINE TRANSPOSES À MATRIX 
у Na DT = ши A e 
DIMENSIONS AND DECLARATIONS 
REALS ВОВ, ВМ 


O OOO a 


DO 100 I= 1.N 
рӯ 1005. = ќе a 
B(J.1) = АСТ) 
100 COS TINUE 


RETURN 


END 


ME he Ar ВЕ z 
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Teas ROUTINE MULTIPLIES ‘A MATRIX WITH A SCALAR 
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DIMENSIONS AND DECLARATIONS 
REAL* 24 Bos EODEM Q 


WX 


XO CO e 


r 
` 


DO 100 I = 1,5 
ромашка а 
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